/*
 *  FinderPatternFinder.cpp
 *  zxing
 *
 *  Created by Christian Brunschen on 13/05/2008.
 *  Copyright 2008 ZXing authors All rights reserved.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *      http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#include "FinderPatternFinder.h"
//#include "../../ReaderException.h"
#include <vector>
#include <cmath>

namespace qrcode {
  namespace detector {
    
    using namespace std;
    using namespace common;
    
    class ClosestToAverageComparator {
    private:
      float averageModuleSize_;
    public:
      ClosestToAverageComparator(float averageModuleSize) :
      averageModuleSize_(averageModuleSize) { }
      int operator()(Ref<FinderPattern> a, Ref<FinderPattern> b) {
        float dA = abs(a->getEstimatedModuleSize() - averageModuleSize_);
        float dB = abs(b->getEstimatedModuleSize() - averageModuleSize_);
        return dA < dB ? -1 : dA > dB ? 1 : 0;
      }
    };

    class CenterComparator {
    public:
      int operator()(Ref<FinderPattern> a, Ref<FinderPattern> b) {
        return b->getCount() - a->getCount();
      }
    };
    
    int FinderPatternFinder::CENTER_QUORUM = 2;
    int FinderPatternFinder::MIN_SKIP = 3;
    int FinderPatternFinder::MAX_MODULES = 57;
    
    float FinderPatternFinder::centerFromEnd(valarray<int> &stateCount,
                                             int end) {
      return (float) (end - stateCount[4] - stateCount[3]) -
        stateCount[2] / 2.0f;
    }
    
    bool FinderPatternFinder::foundPatternCross(valarray<int> &stateCount) {
      int totalModuleSize = 0;
      for (int i = 0; i < 5; i++) {
        if (stateCount[i] == 0) {
          return false;
        }
        totalModuleSize += stateCount[i];
      }
      if (totalModuleSize < 7) {
        return false;
      }
      float moduleSize = (float) totalModuleSize / 7.0f;
      float maxVariance = moduleSize / 2.0f;
      // Allow less than 50% variance from 1-1-3-1-1 proportions
      return abs(moduleSize - stateCount[0]) < maxVariance &&
        abs(moduleSize - stateCount[1]) < maxVariance &&
        abs(3.0f * moduleSize - stateCount[2]) < 3.0f * maxVariance &&
        abs(moduleSize - stateCount[3]) < maxVariance &&
        abs(moduleSize - stateCount[4]) < maxVariance;
    }
    
    float FinderPatternFinder::crossCheckVertical(size_t startI, size_t centerJ, 
                                                  int maxCount,
                                                  int originalStateCountTotal) {
      
      int maxI = image_->getHeight();
      valarray<int> stateCount((const int)0, 5);
      
      // Start counting up from center
      int i = startI;
      while (i >= 0 && image_->isBlack(centerJ, i)) {
        stateCount[2]++;
        i--;
      }
      if (i < 0) {
        return NAN;
      }
      while (i >= 0 && !image_->isBlack(centerJ, i) && stateCount[1] <= maxCount) {
        stateCount[1]++;
        i--;
      }
      // If already too many modules in this state or ran off the edge:
      if (i < 0 || stateCount[1] > maxCount) {
        return NAN;
      }
      while (i >= 0 && image_->isBlack(centerJ, i) && stateCount[0] <= maxCount) {
        stateCount[0]++;
        i--;
      }
      if (stateCount[0] > maxCount) {
        return NAN;
      }
      
      // Now also count down from center
      i = startI + 1;
      while (i < maxI && image_->isBlack(centerJ, i)) {
        stateCount[2]++;
        i++;
      }
      if (i == maxI) {
        return NAN;
      }
      while (i < maxI && !image_->isBlack(centerJ, i) && stateCount[3] < maxCount) {
        stateCount[3]++;
        i++;
      }
      if (i == maxI || stateCount[3] >= maxCount) {
        return NAN;
      }
      while (i < maxI && image_->isBlack(centerJ, i) && stateCount[4] < maxCount) {
        stateCount[4]++;
        i++;
      }
      if (stateCount[4] >= maxCount) {
        return NAN;
      }
      
      // If we found a finder-pattern-like section, but its size is more than 20% different than
      // the original, assume it's a false positive
      int stateCountTotal = stateCount[0] + stateCount[1] + stateCount[2] + stateCount[3] + stateCount[4];
      if (5 * abs(stateCountTotal - originalStateCountTotal) >= originalStateCountTotal) {
        return NAN;
      }
      
      return foundPatternCross(stateCount) ? centerFromEnd(stateCount, i) : NAN;
    }
    
    float FinderPatternFinder::crossCheckHorizontal(size_t startJ, size_t centerI, 
                                                    int maxCount,
                                                    int originalStateCountTotal)
    {
      
      int maxJ = image_->getWidth();
      valarray<int> stateCount((const int)0, 5);
      
      int j = startJ;
      while (j >= 0 && image_->isBlack(j, centerI)) {
        stateCount[2]++;
        j--;
      }
      if (j < 0) {
        return NAN;
      }
      while (j >= 0 && !image_->isBlack(j, centerI) && 
             stateCount[1] <= maxCount) {
        stateCount[1]++;
        j--;
      }
      if (j < 0 || stateCount[1] > maxCount) {
        return NAN;
      }
      while (j >= 0 && image_->isBlack(j, centerI) && 
             stateCount[0] <= maxCount) {
        stateCount[0]++;
        j--;
      }
      if (stateCount[0] > maxCount) {
        return NAN;
      }
      
      j = startJ + 1;
      while (j < maxJ && image_->isBlack(j, centerI)) {
        stateCount[2]++;
        j++;
      }
      if (j == maxJ) {
        return NAN;
      }
      while (j < maxJ && !image_->isBlack(j, centerI) && 
             stateCount[3] < maxCount) {
        stateCount[3]++;
        j++;
      }
      if (j == maxJ || stateCount[3] >= maxCount) {
        return NAN;
      }
      while (j < maxJ && image_->isBlack(j, centerI) && stateCount[4] < maxCount) {
        stateCount[4]++;
        j++;
      }
      if (stateCount[4] >= maxCount) {
        return NAN;
      }
      
      // If we found a finder-pattern-like section, but its size is significantly different than
      // the original, assume it's a false positive
      int stateCountTotal = stateCount[0] + stateCount[1] + stateCount[2] + 
      stateCount[3] + stateCount[4];
      if (5 * abs(stateCountTotal - originalStateCountTotal) >= 
          originalStateCountTotal) {
        return NAN;
      }
      
      return foundPatternCross(stateCount) ? centerFromEnd(stateCount, j) : NAN;
    }
    
    bool FinderPatternFinder::handlePossibleCenter(valarray<int> &stateCount, 
                                                   size_t i, size_t j) {
      int stateCountTotal = stateCount[0] + stateCount[1] + stateCount[2] + 
        stateCount[3] + stateCount[4];
      float centerJ = centerFromEnd(stateCount, j);
      float centerI = crossCheckVertical(i, centerJ, stateCount[2],
                                         stateCountTotal);
      if (!isnan(centerI)) {
        // Re-cross check
        centerJ = crossCheckHorizontal(centerJ, centerI, stateCount[2],
                                       stateCountTotal);
        if (!isnan(centerJ)) {
          float estimatedModuleSize = (float) stateCountTotal / 7.0f;
          bool found = false;
          size_t max = possibleCenters_.size();
          for (size_t index = 0; index < max; index++) {
            Ref<FinderPattern> center = possibleCenters_[index];
            // Look for about the same center and module size:
            if (center->aboutEquals(estimatedModuleSize, centerI, centerJ)) {
              center->incrementCount();
              found = true;
              break;
            }
          }
          if (!found) {
            Ref<FinderPattern> newPattern
              (new FinderPattern(centerJ, centerI, 
                                 estimatedModuleSize));
            possibleCenters_.push_back(newPattern);
          }
          return true;
        }
      }
      return false;
    }
    
    int FinderPatternFinder::findRowSkip() {
      size_t max = possibleCenters_.size();
      if (max <= 1) {
        return 0;
      }
      Ref<FinderPattern> firstConfirmedCenter;
      for (size_t i = 0; i < max; i++) {
        Ref<FinderPattern> center = possibleCenters_[i];
        if (center->getCount() >= CENTER_QUORUM) {
          if (firstConfirmedCenter == 0) {
            firstConfirmedCenter = center;
          } else {
            // We have two confirmed centers
            // How far down can we skip before resuming looking for the next
            // pattern? In the worst case, only the difference between the
            // difference in the x / y coordinates of the two centers.
            // This is the case where you find top left first. Draw it out.
            hasSkipped_ = true;
            return (int) (abs(firstConfirmedCenter->getX() - center->getX()) -
                          abs(firstConfirmedCenter->getY() - center->getY()));
          }
        }
      }
      return 0;
    }
    
    bool FinderPatternFinder::haveMultiplyConfirmedCenters() {
      int confirmedCount = 0;
      float totalModuleSize = 0.0f;
      size_t max = possibleCenters_.size();
      for (size_t i = 0; i < max; i++) {
        Ref<FinderPattern> pattern = possibleCenters_[i];
        if (pattern->getCount() >= CENTER_QUORUM) {
          confirmedCount++;
          totalModuleSize += pattern->getEstimatedModuleSize();
        }
      }
      if (confirmedCount < 3) {
        return false;
      }
      // OK, we have at least 3 confirmed centers, but, it's possible that one is a "false positive"
      // and that we need to keep looking. We detect this by asking if the estimated module sizes
      // vary too much. We arbitrarily say that when the total deviation from average exceeds
      // 15% of the total module size estimates, it's too much.
      float average = totalModuleSize / max;
      float totalDeviation = 0.0f;
      for (size_t i = 0; i < max; i++) {
        Ref<FinderPattern> pattern = possibleCenters_[i];
        totalDeviation += abs(pattern->getEstimatedModuleSize() - average);
      }
      return totalDeviation <= 0.15f * totalModuleSize;
    }
    
    int FinderPatternFinder::selectBestPatterns() {
      sort(possibleCenters_.begin(), possibleCenters_.end(),
           CenterComparator());
      size_t size = 0;
      size_t max = possibleCenters_.size();
      while (size < max) {
        if (possibleCenters_[size]->getCount() < CENTER_QUORUM) {
          break;
        }
        size++;
      }
      
      if (size < 3) {
        // Couldn't find enough finder patterns
//        throw new ReaderException("Could not find three finder patterns");
    	  return -1;
      }
      
      if (size == 3) {
        // Found just enough -- hope these are good!
        Ref<FinderPattern> rawResult[] = {
          possibleCenters_[0], possibleCenters_[1], possibleCenters_[2]
        };
        ArrayRef<Ref<FinderPattern> > result(rawResult, 3);
        bestPatterns_ = result;
        return 0;
      }
      
      // Hmm, multiple found. We need to pick the best three. Find the most
      // popular ones whose module size is nearest the average
      float averageModuleSize = 0.0f;
      for (size_t i = 0; i < size; i++) {
        averageModuleSize += possibleCenters_[i]->getEstimatedModuleSize();
      }
      averageModuleSize /= (float) size;
      
      sort(possibleCenters_.begin(), possibleCenters_.end(), 
           ClosestToAverageComparator(averageModuleSize));
      
      Ref<FinderPattern> rawResult[] = {
        possibleCenters_[0], possibleCenters_[1], possibleCenters_[2]
      };
      ArrayRef<Ref<FinderPattern> > result(rawResult, 3);
      bestPatterns_ = result;
      return 0;
    }
    
    ArrayRef<Ref<FinderPattern> > FinderPatternFinder::orderBestPatterns
    (ArrayRef<Ref<FinderPattern> > patterns) {
      // Find distances between pattern centers
      float abDistance = distance(patterns[0], patterns[1]);
      float bcDistance = distance(patterns[1], patterns[2]);
      float acDistance = distance(patterns[0], patterns[2]);
      
      Ref<FinderPattern> topLeft;
      Ref<FinderPattern> topRight;
      Ref<FinderPattern> bottomLeft;
      // Assume one closest to other two is top left;
      // topRight and bottomLeft will just be guesses below at first
      if (bcDistance >= abDistance && bcDistance >= acDistance) {
        topLeft = patterns[0];
        topRight = patterns[1];
        bottomLeft = patterns[2];
      } else if (acDistance >= bcDistance && acDistance >= abDistance) {
        topLeft = patterns[1];
        topRight = patterns[0];
        bottomLeft = patterns[2];
      } else {
        topLeft = patterns[2];
        topRight = patterns[0];
        bottomLeft = patterns[1];
      }
      
      // Use cross product to figure out which of other1/2 is the bottom left
      // pattern. The vector "top-left -> bottom-left" x "top-left -> top-right"
      // should yield a vector with positive z component
      if ((bottomLeft->getY() - topLeft->getY()) * 
          (topRight->getX() - topLeft->getX()) <
          (bottomLeft->getX() - topLeft->getX()) * 
          (topRight->getY() - topLeft->getY())) {
        Ref<FinderPattern> temp = topRight;
        topRight = bottomLeft;
        bottomLeft = temp;
      }
      
      ArrayRef<Ref<FinderPattern> > results(3);
      results[0] = bottomLeft;
      results[1] = topLeft;
      results[2] = topRight;
      return results;
    }
    
    float FinderPatternFinder::distance(Ref<ResultPoint> p1, 
                                        Ref<ResultPoint> p2) {
      float dx = p1->getX() - p2->getX();
      float dy = p1->getY() - p2->getY();
      return (float)sqrt(dx*dx + dy*dy);
    }
    
    FinderPatternFinder::FinderPatternFinder(Ref<MonochromeBitmapSource> image)
    : image_(image), possibleCenters_(), hasSkipped_(false) { 
    }
    
    int FinderPatternFinder::find() {
      size_t maxI = image_->getHeight();
      size_t maxJ = image_->getWidth();
      
      // We are looking for black/white/black/white/black modules in
      // 1:1:3:1:1 ratio; this tracks the number of such modules seen so far
      valarray<int> stateCount(5);
      bool done = false;
      
      // Let's assume that the maximum version QR Code we support takes up 1/4
      // the height of the image, and then account for the center being 3
      // modules in size. This gives the smallest number of pixels the center
      // could be, so skip this often. When trying harder, look for all
      // QR versions regardless of how dense they are.
      size_t iSkip = MIN_SKIP;
      
      for (size_t i = iSkip - 1; i < maxI && !done; i += iSkip) {
        // Get a row of black/white values
        Ref<BitArray> null;
        Ref<BitArray> blackRow = image_->getBlackRow(i, null, 0, maxJ);
        stateCount[0] = 0;
        stateCount[1] = 0;
        stateCount[2] = 0;
        stateCount[3] = 0;
        stateCount[4] = 0;
        int currentState = 0;
        for (size_t j = 0; j < maxJ; j++) {
          if (blackRow->get(j)) {
            // Black pixel
            if ((currentState & 1) == 1) { // Counting white pixels
              currentState++;
            }
            stateCount[currentState]++;
          } else { // White pixel
            if ((currentState & 1) == 0) { // Counting black pixels
              if (currentState == 4) { // A winner?
                if (foundPatternCross(stateCount)) { // Yes
                  bool confirmed = handlePossibleCenter(stateCount, i, j);
                  if (confirmed) {
                    iSkip = 1; // Go back to examining each line
                    if (hasSkipped_) {
                      done = haveMultiplyConfirmedCenters();
                    } else {
                      int rowSkip = findRowSkip();
                      if (rowSkip > stateCount[2]) {
                        // Skip rows between row of lower confirmed center
                        // and top of presumed third confirmed center
                        // but back up a bit to get a full chance of detecting
                        // it, entire width of center of finder pattern
                        
                        // Skip by rowSkip, but back off by stateCount[2] (size
                        // of last center of pattern we saw) to be conservative,
                        // and also back off by iSkip which is about to be 
                        // re-added
                        i += rowSkip - stateCount[2] - iSkip;
                        j = maxJ - 1;
                      }
                    }
                  } else {
                    // Advance to next black pixel
                    do {
                      j++;
                    } while (j < maxJ && !blackRow->get(j));
                    j--; // back up to that last white pixel
                  }
                  // Clear state to start looking again
                  currentState = 0;
                  stateCount[0] = 0;
                  stateCount[1] = 0;
                  stateCount[2] = 0;
                  stateCount[3] = 0;
                  stateCount[4] = 0;
                } else { // No, shift counts back by two
                  stateCount[0] = stateCount[2];
                  stateCount[1] = stateCount[3];
                  stateCount[2] = stateCount[4];
                  stateCount[3] = 1;
                  stateCount[4] = 0;
                  currentState = 3;
                }
              } else {
                stateCount[++currentState]++;
              }
            } else { // Counting white pixels
              stateCount[currentState]++;
            }
          }
        }
        if (foundPatternCross(stateCount)) {
          bool confirmed = handlePossibleCenter(stateCount, i, maxJ);
          if (confirmed) {
            iSkip = stateCount[0];
            if (hasSkipped_) {
              // Found a third one
              done = haveMultiplyConfirmedCenters();
            }
          }
        }
      }
      
      int error = selectBestPatterns();
      if (error != 0) {
    	  return error;
      }
      ArrayRef<Ref<FinderPattern> > patternInfo = bestPatterns_;
      patternInfo = orderBestPatterns(patternInfo);
      
      Ref<FinderPatternInfo> result(new FinderPatternInfo(patternInfo));
      result_ = result;
      return 0;
    }
  }
}
